#include "tim.h"
#include "motor.h"


void SetMotorDirection(GPIO_TypeDef* port, uint16_t pin, uint8_t state)
{
    if(state) HAL_GPIO_WritePin(port, pin, GPIO_PIN_SET);
    else HAL_GPIO_WritePin(port, pin, GPIO_PIN_RESET);
}

void SetPWMDutyCycle(TIM_HandleTypeDef* htim, uint32_t channel, uint16_t duty)
{
//    duty = (htim->Init.Period + 1) * duty / 100;
    if(htim->Instance == TIM3)
    {
        if(channel == TIM_CHANNEL_1) __HAL_TIM_SetCompare(htim, TIM_CHANNEL_1, duty);
        else if(channel == TIM_CHANNEL_2) __HAL_TIM_SetCompare(htim, TIM_CHANNEL_2, duty); 
    }
    else if (htim->Instance == TIM4)
    {
        if(channel == TIM_CHANNEL_1) __HAL_TIM_SetCompare(htim, TIM_CHANNEL_1, duty);
        else if(channel == TIM_CHANNEL_2) __HAL_TIM_SetCompare(htim, TIM_CHANNEL_2, duty); 
    }
}

void SetChassisSpeedByCCR(uint16_t ChassisCCR) // full speed is 999
{
    SetPWMDutyCycle(&htim3,TIM_CHANNEL_1,ChassisCCR);
    SetPWMDutyCycle(&htim3,TIM_CHANNEL_2,ChassisCCR);
    SetPWMDutyCycle(&htim4,TIM_CHANNEL_1,ChassisCCR);
    SetPWMDutyCycle(&htim4,TIM_CHANNEL_2,ChassisCCR);
}

// 此处为开环控制

void SetMotorDirectionForward()
{
    SetMotorDirection(L298N1_IN1_GPIO_Port, L298N1_IN1_Pin, 0);
    SetMotorDirection(L298N1_IN2_GPIO_Port, L298N1_IN2_Pin, 1);
    SetMotorDirection(L298N1_IN1_GPIO_Port, L298N1_IN1_Pin, 0);
    SetMotorDirection(L298N1_IN2_GPIO_Port, L298N1_IN2_Pin, 1);
    SetMotorDirection(L298N1_IN3_GPIO_Port, L298N1_IN3_Pin, 0);
    SetMotorDirection(L298N1_IN4_GPIO_Port, L298N1_IN4_Pin, 1);
    SetMotorDirection(L298N2_IN1_GPIO_Port, L298N2_IN1_Pin, 1);
    SetMotorDirection(L298N2_IN2_GPIO_Port, L298N2_IN2_Pin, 0);
    SetMotorDirection(L298N2_IN3_GPIO_Port, L298N2_IN3_Pin, 0);
    SetMotorDirection(L298N2_IN4_GPIO_Port, L298N2_IN4_Pin, 1);
}
 
void SetMotorDirectionBackward()
{
    SetMotorDirection(L298N1_IN1_GPIO_Port, L298N1_IN1_Pin, 0);
    SetMotorDirection(L298N1_IN2_GPIO_Port, L298N1_IN2_Pin, 1);
    SetMotorDirection(L298N1_IN3_GPIO_Port, L298N1_IN3_Pin, 1);
    SetMotorDirection(L298N1_IN4_GPIO_Port, L298N1_IN4_Pin, 0);
    SetMotorDirection(L298N2_IN1_GPIO_Port, L298N1_IN1_Pin, 0);
    SetMotorDirection(L298N2_IN2_GPIO_Port, L298N2_IN2_Pin, 1);
    SetMotorDirection(L298N2_IN3_GPIO_Port, L298N2_IN3_Pin, 1);
    SetMotorDirection(L298N2_IN4_GPIO_Port, L298N2_IN4_Pin, 0);
}

void SetMotorDirectionTurnLeft()
{
    SetMotorDirection(L298N1_IN1_GPIO_Port, L298N1_IN1_Pin, 0);
    SetMotorDirection(L298N1_IN2_GPIO_Port, L298N1_IN2_Pin, 1);
    SetMotorDirection(L298N1_IN3_GPIO_Port, L298N1_IN3_Pin, 0);
    SetMotorDirection(L298N1_IN4_GPIO_Port, L298N1_IN4_Pin, 1);
    SetMotorDirection(L298N2_IN1_GPIO_Port, L298N2_IN1_Pin, 0);
    SetMotorDirection(L298N2_IN2_GPIO_Port, L298N2_IN2_Pin, 1);
    SetMotorDirection(L298N2_IN3_GPIO_Port, L298N2_IN3_Pin, 0);
    SetMotorDirection(L298N2_IN4_GPIO_Port, L298N2_IN4_Pin, 1);
}
 
void SetMotorDirectionTurnRight()
{
    SetMotorDirection(L298N1_IN1_GPIO_Port, L298N1_IN1_Pin, 1);
    SetMotorDirection(L298N1_IN2_GPIO_Port, L298N1_IN2_Pin, 0);
    SetMotorDirection(L298N1_IN3_GPIO_Port, L298N1_IN3_Pin, 1);
    SetMotorDirection(L298N1_IN4_GPIO_Port, L298N1_IN4_Pin, 0);
    SetMotorDirection(L298N2_IN1_GPIO_Port, L298N2_IN1_Pin, 1);
    SetMotorDirection(L298N2_IN2_GPIO_Port, L298N2_IN2_Pin, 0);
    SetMotorDirection(L298N2_IN3_GPIO_Port, L298N2_IN3_Pin, 1);
    SetMotorDirection(L298N2_IN4_GPIO_Port, L298N2_IN4_Pin, 0);
}
 
void SetMotorDirectionLeftRevolve()
{
    SetMotorDirection(L298N1_IN1_GPIO_Port, L298N1_IN1_Pin, 0);
    SetMotorDirection(L298N1_IN2_GPIO_Port, L298N1_IN2_Pin, 1);
    SetMotorDirection(L298N1_IN3_GPIO_Port, L298N1_IN3_Pin, 0);
    SetMotorDirection(L298N1_IN4_GPIO_Port, L298N1_IN4_Pin, 1);
    SetMotorDirection(L298N2_IN1_GPIO_Port, L298N2_IN1_Pin, 0);
    SetMotorDirection(L298N2_IN2_GPIO_Port, L298N2_IN2_Pin, 1);
    SetMotorDirection(L298N2_IN3_GPIO_Port, L298N2_IN3_Pin, 0);
    SetMotorDirection(L298N2_IN4_GPIO_Port, L298N2_IN4_Pin, 1);
}
 
void SetMotorDirectionRightRevolve()
{
    SetMotorDirection(L298N1_IN1_GPIO_Port, L298N1_IN1_Pin, 1);
    SetMotorDirection(L298N1_IN2_GPIO_Port, L298N1_IN2_Pin, 0);
    SetMotorDirection(L298N1_IN3_GPIO_Port, L298N1_IN3_Pin, 1);
    SetMotorDirection(L298N1_IN4_GPIO_Port, L298N1_IN4_Pin, 0);
    SetMotorDirection(L298N2_IN1_GPIO_Port, L298N2_IN1_Pin, 1);
    SetMotorDirection(L298N2_IN2_GPIO_Port, L298N2_IN2_Pin, 0);
    SetMotorDirection(L298N2_IN3_GPIO_Port, L298N2_IN3_Pin, 1);
    SetMotorDirection(L298N2_IN4_GPIO_Port, L298N2_IN4_Pin, 0);
}
